﻿using System;
using System.Collections.Generic;
using UnityEngine;
using NeuronDataReaderWraper;

namespace Neuron
{
	public class CustomSkelTransformsInstance : NeuronInstance
	{	
		public Transform					root = null;
		public string						prefix = "Robot_";
		public bool							boundTransforms { get ; private set; }
		public bool							physicalUpdate = false;

		public TextAsset 					configFile;
		CustomSkelConfig					config = new CustomSkelConfig();
        Transform[]                         extraTransforms = null;

		public Transform[]					transforms = new Transform[(int)NeuronBones.NumOfBones];
		CustomSkelTransformsPhysicalReference	physicalReference = new CustomSkelTransformsPhysicalReference();
		Vector3[]							bonePositionOffsets = new Vector3[(int)NeuronBones.NumOfBones];
		Vector3[]							boneRotationOffsets = new Vector3[(int)NeuronBones.NumOfBones];

		public CustomSkelTransformsInstance()
		{
		}

		public CustomSkelTransformsInstance( string address, int port, int commandServerPort, NeuronConnection.SocketType socketType, int actorID )
			:base( address, port, commandServerPort, socketType, actorID )
		{
		}

		public CustomSkelTransformsInstance( Transform root, string prefix, string address, int port, int commandServerPort, NeuronConnection.SocketType socketType, int actorID )
			:base( address, port, commandServerPort, socketType, actorID )
		{
			Bind( root, prefix );
		}

		public CustomSkelTransformsInstance( Transform root, string prefix, NeuronActor actor )
			:base( actor )
		{
			Bind( root, prefix );
		}

		public CustomSkelTransformsInstance( NeuronActor actor )
			:base( actor )
		{
		}

		new void OnEnable()
		{
			base.OnEnable();

			if( root == null )
			{
				root = transform;
			}

			Bind( root, prefix );
		}

		new void Update()
		{
			base.ToggleConnect();
			base.Update();

			if( boundActor != null && boundTransforms && !physicalUpdate )
			{				
				if( physicalReference.Initiated() )
				{
					ReleasePhysicalContext();
				}

				ApplyMotion( boundActor, transforms, extraTransforms, bonePositionOffsets, boneRotationOffsets, config );
			}
		}

		void FixedUpdate()
		{
			base.ToggleConnect();

			if( boundActor != null && boundTransforms && physicalUpdate )
			{				
				if( !physicalReference.Initiated() )
				{
					physicalUpdate = InitPhysicalContext();
				}

				ApplyMotionPhysically( physicalReference.GetReferenceTransforms(), transforms );
			}
		}

		public Transform[] GetTransforms()
		{
			return transforms;
		}

		static bool ValidateVector3( Vector3 vec )
		{
			return !float.IsNaN( vec.x ) && !float.IsNaN( vec.y ) && !float.IsNaN( vec.z )
				&& !float.IsInfinity( vec.x ) && !float.IsInfinity( vec.y ) && !float.IsInfinity( vec.z );
		}

		// set position for bone
		static void SetPosition( Transform[] transforms, int bone, Vector3 pos )
		{
			Transform t = transforms[bone];
			if( t != null )
			{
				// calculate position when we have scale
				pos.Scale( new Vector3( 1.0f / t.parent.lossyScale.x, 1.0f / t.parent.lossyScale.y, 1.0f / t.parent.lossyScale.z ) );

				if( !float.IsNaN( pos.x ) && !float.IsNaN( pos.y ) && !float.IsNaN( pos.z ) )
				{
					t.localPosition = pos;
				}
			}
		}

		// set rotation for bone
		static void SetRotation( Transform[] transforms, int bone, Vector3 rotation )
		{
			Transform t = transforms[bone];
			if( t != null )
			{
				Quaternion rot = Quaternion.Euler( rotation );
				if( !float.IsNaN( rot.x ) && !float.IsNaN( rot.y ) && !float.IsNaN( rot.z ) && !float.IsNaN( rot.w ) )
				{
					t.localRotation = rot;
				}
			}
		}

        static void SetRotation(Transform[] transforms, int bone, Quaternion rotation)
        {
            Transform t = transforms[bone];
            if (t != null)
            {
                if (!float.IsNaN(rotation.x) &&
                    !float.IsNaN(rotation.y) &&
                    !float.IsNaN(rotation.z) &&
                    !float.IsNaN(rotation.w))
                {
                    t.localRotation = rotation;
                }
            }
        }

        // apply transforms extracted from actor mocap data to bones
        public static void ApplyMotion( NeuronActor actor, Transform[] transforms, Transform[] extraTransforms, Vector3[] bonePositionOffsets, Vector3[] boneRotationOffsets, CustomSkelConfig config )
		{			
			// apply Hips position
			SetPosition( transforms, (int)NeuronBones.Hips, actor.GetReceivedPosition( NeuronBones.Hips ) );
			SetRotation( transforms, (int)NeuronBones.Hips, actor.GetReceivedRotation( NeuronBones.Hips ) );

			// apply positions
			if( actor.withDisplacement )
			{
				for( int i = 1; i < (int)NeuronBones.NumOfBones && i < transforms.Length; ++i )
				{
					NeuronBones bone = (NeuronBones)i;

					if (i < (int)NeuronBones.Spine) {
						SetPosition( transforms, i, actor.GetReceivedPosition( bone ) + bonePositionOffsets[i] );
					}
						
					Vector3 rot = actor.GetReceivedRotation (bone) + boneRotationOffsets [i];

					CustomSkelConfigStrategy1 s1 = config.strategy_1;
					if (!s1.rules.ContainsKey (i)) {
						SetRotation (transforms, i, rot);
					} else {
						//Debug.Log (string.Format ("Standard Bone: {0}       Rot: {1}", bone, rot));
						Quaternion quat_origin = Quaternion.Euler (rot);
						var rule = s1.rules [i];
						var iter = rule.ruleUnits.GetEnumerator ();
						while (iter.MoveNext ()) {
							if (iter.Current.bone == i) {
								Quaternion quat_childbones = Quaternion.AngleAxis ((1.0f - iter.Current.weight) * rot.x, Vector3.right);
								Quaternion quat_bone = quat_origin * Quaternion.Inverse (quat_childbones);
								SetRotation (transforms, iter.Current.bone, quat_bone);
							} else {
								Vector3 _rot = new Vector3 (iter.Current.weight * rot.x, 0.0f, 0.0f);
								SetRotation (extraTransforms, iter.Current.bone-CustomSkelHelper.IdGeneratorBase, _rot);
							}
						}
					}

					CustomSkelConfigStrategy2 s2 = config.strategy_2;
					if (s2.constraints.ContainsKey (i)) {
						var constraint = s2.constraints [i];
						var controller = s2.controller [i];
						float yRot = constraint.rootCtrlPointAction (rot.y);
                        SetRotation(extraTransforms, controller.rootBone-CustomSkelHelper.IdGeneratorBase, new Vector3(0,yRot,0));
                        for (int s = 0; s < controller.subBones.Count; s++) {
                            SetPosition(extraTransforms, controller.subBones[s].bone-CustomSkelHelper.IdGeneratorBase, constraint.subCtrlPointAction[s](yRot));
                        }
                    }
				}
			}
			else
			{
				// apply rotations
				for( int i = 1; i < (int)NeuronBones.NumOfBones && i < transforms.Length; ++i )
				{
					NeuronBones bone = (NeuronBones)i;

                    Vector3 rot = actor.GetReceivedRotation (bone);

					CustomSkelConfigStrategy1 s1 = config.strategy_1;
					if (!s1.rules.ContainsKey (i)) {
						SetRotation (transforms, i, rot);
					} else {
						Quaternion quat_origin = Quaternion.Euler (rot);
						var rule = s1.rules [i];
						var iter = rule.ruleUnits.GetEnumerator ();
						while (iter.MoveNext ()) {
							if (iter.Current.bone == i) {
								Quaternion quat_childbones = Quaternion.AngleAxis ((1.0f - iter.Current.weight) * rot.x, Vector3.right);
								Quaternion quat_bone = quat_origin * Quaternion.Inverse (quat_childbones);
								SetRotation (transforms, iter.Current.bone, quat_bone);
							} else {
								Vector3 _rot = new Vector3 (iter.Current.weight * rot.x, 0.0f, 0.0f);
								SetRotation (extraTransforms, iter.Current.bone-CustomSkelHelper.IdGeneratorBase, _rot);
							}
						}
					}

					CustomSkelConfigStrategy2 s2 = config.strategy_2;
					if (s2.constraints.ContainsKey (i)) {
						var constraint = s2.constraints [i];
						var controller = s2.controller [i];
						float yRot = constraint.rootCtrlPointAction (rot.y);
						SetRotation (extraTransforms, controller.rootBone-CustomSkelHelper.IdGeneratorBase, new Vector3 (0.0f, yRot, 0.0f));
						for (int s = 0; s < controller.subBones.Count; s++) {
							SetPosition (extraTransforms, controller.subBones [s].bone-CustomSkelHelper.IdGeneratorBase, constraint.subCtrlPointAction [s] (yRot));
						}
					}
				}
			}
		}

		// apply Transforms of src bones to dest Rigidbody Components of bone
		public static void ApplyMotionPhysically( Transform[] src, Transform[] dest )
		{
			if( src != null && dest != null )
			{
				for( int i = 0; i < (int)NeuronBones.NumOfBones; ++i )
				{
					Transform src_transform = src[i];
					Transform dest_transform = dest[i];
					if( src_transform != null && dest_transform != null )
					{
						Rigidbody rigidbody = dest_transform.GetComponent<Rigidbody>();
						if( rigidbody != null )
						{
							rigidbody.MovePosition( src_transform.position );
							rigidbody.MoveRotation( src_transform.rotation );
						}
					}
				}
			}
		}

        public void ParseConfigFile()
        {
            Debug.Log("****************** Begin parse CustomSkelConfig ********************");
            Debug.Log(configFile.name);
            if (config.parse(configFile.text))
            {
                Debug.Log("Parse successfully.");
            }
            Debug.Log("****************** End parse CustomSkelConfig ********************");

            if (CustomSkelHelper.IsExistExtraBone)
            {
                extraTransforms = new Transform[CustomSkelHelper.ExtraBoneCount];
            }
        }
        
		public bool BindStandardBone( Transform root, string prefix )
		{
			bool bindOk = CustomSkelHelper.BindStandardBone( root, transforms, prefix, false );
			UpdateOffset();
			return bindOk;
		}

        public bool BindExtraBone(Transform root, string prefix)
        {
            int count = CustomSkelHelper.BindExtraBone(root, extraTransforms, prefix, false);
            return true;
        }

        public bool Bind(Transform root, string prefix)
        {
            ParseConfigFile();

            this.root = root;
            this.prefix = prefix;

            boundTransforms =
                BindStandardBone(root, prefix) &&
                BindExtraBone(root, prefix);

            return boundTransforms;
        }

        bool InitPhysicalContext()
		{
			if( physicalReference.Init( root, prefix, transforms ) )
			{
				// break original object's hierachy of transforms, so we can use MovePosition() and MoveRotation() to set transform
				NeuronHelper.BreakHierarchy( transforms );
				return true;
			}

			return false;
		}

		void ReleasePhysicalContext()
		{
			physicalReference.Release();
		}

		void UpdateOffset()
		{
			// initiate values
			for( int i = 0; i < (int)HumanBodyBones.LastBone; ++i )
			{
				bonePositionOffsets[i] = Vector3.zero;
				boneRotationOffsets[i] = Vector3.zero;
			}

			if( boundTransforms )
			{
				bonePositionOffsets[(int)NeuronBones.LeftUpLeg] = new Vector3( 0.0f, transforms[(int)NeuronBones.LeftUpLeg].localPosition.y, 0.0f );
				bonePositionOffsets[(int)NeuronBones.RightUpLeg] = new Vector3( 0.0f, transforms[(int)NeuronBones.RightUpLeg].localPosition.y, 0.0f );
			}
		}
	}
}